#include "CubicLine.h"
#include "../../constants/MathConstants.h"
#include <algorithm>
#include "../../_Matrix.h"
namespace OpenDrive{

	CubicLine::CubicLine(){}

	CubicLine::CubicLine(const Point &start, const Point &end){
		this->start = start;
		this->end = end;
	}

	CubicLine::~CubicLine() {}

	std::vector<Point> CubicLine::calPoints()	{
		double te = sqrt(pow(start.getX() - end.getX(), 2) + pow(start.getY() - end.getY(), 2));
		std::vector<float> px, py;
		//计算参数
		calPara(te, px, py);
		float x, y;
		std::vector<Point> res;
		for (float t = 0; t < te; t += 0.5) {
			x = px[0] + t * px[1] + t * t*px[2] + t * t*t*px[3];
			y = py[0] + t * py[1] + t * t*py[2] + t * t*t*py[3];
			Point temp(x, y, 0, 0);
			res.push_back(temp);
		}
		float t = te;
		x = px[0] + t * px[1] + t * t*px[2] + t * t*t*px[3];
		y = py[0] + t * py[1] + t * t*py[2] + t * t*t*py[3];
		Point temp(x, y, 0, 0);
		res.push_back(temp);
		//计算角度
		int size = res.size();
		for (int i = 1; i < size; i++) {
			res[i].setTheta(atan2(res[i].getY() - res[i - 1].getY(),res[i].getX() - res[i - 1].getX()));
		}
		res[0] = res[1];
		return std::move(res);	
	}

	Point& CubicLine::getStart() {	return start;}

	Point& CubicLine::getEnd() {  return end;	}

	void CubicLine::setStart(const Point & start) {
		this->start = start;
	}
	
	void CubicLine::setEnd(const Point & end) {
		this->end = end;
	}

	void CubicLine::calPara(float te, std::vector<float>& px, std::vector<float>& py)	{
		//m = [1 0 0 0; 1 te te ^ 2 te ^ 3; 0 1 0 0; 0 1 2 * te 3 * te ^ 2];
		//ax = [x0 x1 cos(theta0) cos(theta1)]';
		//ay = [y0 y1 sin(theta0) sin(theta1)]';
		//px = m\ax;
		//py = m\ay;
		_Matrix A(2, 2);
		A.init_matrix();
		A.write(0, 0, pow(te, 3));
		A.write(0, 1, pow(te, 2));
		A.write(1, 0, pow(te, 2) * 3);
		A.write(1, 1, te * 2);

		_Matrix b1(2, 1);
		b1.init_matrix();
		b1.write(0, 0, end.getX() - start.getX() - cos(start.getTheta())*te);
		b1.write(1, 0, cos(end.getTheta()) - cos(start.getTheta()));

		_Matrix invA(2, 2);
		invA.init_matrix();
		_Matrix param1(2, 1);
		param1.init_matrix();
		_Matrix_Calc c;
		c.inverse(&A, &invA);
		c.multiply(&invA, &b1, &param1);
		//x的参数依次，abcd，a+bt+ct^2+dt^3;
		px.push_back(start.getX());
		px.push_back(cos(start.getTheta()));
		px.push_back(param1.read(1, 0));
		px.push_back(param1.read(0, 0));

		_Matrix b2(2, 1);
		b2.init_matrix();
		b2.write(0, 0, end.getY() - start.getY() - sin(start.getTheta())*te);
		b2.write(1, 0, sin(end.getTheta()) - sin(start.getTheta()));

		_Matrix param2(2, 1);
		param2.init_matrix();
		c.multiply(&invA, &b2, &param2);

		//y的参数依次，abcd，a+bt+ct^2+dt^3;
		py.push_back(start.getY());
		py.push_back(sin(start.getTheta()));
		py.push_back(param2.read(1, 0));
		py.push_back(param2.read(0, 0));
		
		A.free_matrix();
		b1.free_matrix();
		invA.free_matrix();
		param1.free_matrix();
		b2.free_matrix();
		param2.free_matrix();
	}
}